#ifndef CLQR_MODEL1_H
#define CLQR_MODEL1_H

#include <iostream>
#include <fstream>
#include <sstream>
#include <vector>
#include <cmath>

#include "armadillo"
#include "matplotlibcpp.h"

// 主要参考 https://yunchengjiang.blog.csdn.net/article/details/103458473?spm=1001.2014.3001.5502
using namespace std;
using namespace arma;
namespace plt = matplotlibcpp;

typedef struct {
    double delta;
    uword index;
    double e;
    double th_e;
} lqrResult_t;

typedef struct {
    uword index;
    double latError;
} minLatError_idx_t;

const double deg2rad = M_PI/180;
const double rad2deg = 180/M_PI;
const double toKMperH = 3.6;
const double tompers = 1/3.6;

class CLQR_MODEL1
{
public:
    // 总体跟踪效果：在参考路径点附近存在抖动
    double Kp; // 过大不跟踪了
    double dt;
    double L;
    double targetSpeed; // m/s
    double maxSteer; // rad

    mat Q, R;

    vector<double> XRef;
    vector<double> YRef;
    mat refData;

    double pe;
    double pth_e;

    // 初始Y值 过大 跟踪效果不佳
    vec initStates; // X Y yaw v
    vec states;
    mat statesActual;

    uword index;

public:
    CLQR_MODEL1();
    ~CLQR_MODEL1();

    inline double normFunc(double posRefX, double posRefY, double posX, double posY);

    // 生成参考轨迹
    mat generateRefTra(vector<double> &XRef, vector<double> &YRef);

    vec toPiVec(vec angle);
    double toPi(double angle);

    minLatError_idx_t findMinLatError_idx(vec states, mat refData);

    rowvec dlqr(mat A, mat B, mat Q, mat R);

    lqrResult_t lqrSteerControl(vec states, mat refData,
                                double pe, double pth_e, double L,
                                mat Q, mat R, double dt);

    vec updateStates(vec states, double a, double delta,
                     double dt, double L, double maxSteer);

    //void plotRes(vector<double> XRef, vector<double> YRef, vector<double> XActual, vector<double> YActual);
    void plotRes(vector<double> XRef, vector<double> YRef, mat statesActual);

};

#endif // CLQR_MODEL1_H
